/**
 * ...
 * @author Krzysztof Rozalski
 */

package hapi.physics.integration.test;
import hapi.math.MathUtil;
import hapi.math.Vector2D;

typedef State = {
	var position:Vector2D;
	var velocity:Vector2D;
}

typedef Derivative = {
	var velocity:Vector2D;
	var acceleration:Vector2D;
}

class IntegrationBasic {
	
	static function accelerationCar(state:State, t:Float) {
		var maxV = 220;
		var maxDelta = 10;
		var l:Float = 0.0;
		var svl = state.velocity.getLength();
		if ( svl < maxV )
			l = ((maxV - svl) / maxV) * maxDelta;
		return state.velocity.getNormal().mult(l);
	}
	
	static function accelerationCarConst(state:State, t:Float) {
		return new Vector2D(10, 0);
	}
	
	static function acceleration(state:State, t:Float) {
		return accelerationCarConst(state, t);
	}

	static function evaluateWithoutDerivative(initial:State, t:Float):Derivative {
		var output:Derivative = {
			velocity : initial.velocity.clone(),
			acceleration : acceleration(initial, t)
		};
		return output;
	}

	static function evaluate(initial:State, t:Float, dt:Float, d:Derivative):Derivative {
		var state:State = {
			position: d.velocity.multNew(dt).plus(initial.position),
			velocity: d.acceleration.multNew(dt).plus(initial.velocity)
		};
		var output:Derivative = {
			velocity : state.velocity.clone(),
			acceleration : acceleration(state, t + dt)
		};
		return output;
	}
	
	static inline function calcTaylor( a, b, c, d ):Vector2D {
		var twoTimesBPlusC = b.plus(c).mult(2);
		var v = a.plus( twoTimesBPlusC ).plus( d );
		return v.mult(1/6);
	}

	static function integrateRK4(state:State, dt:Float, t:Float) {
		var a = evaluateWithoutDerivative(state, t);
		var b = evaluate(state, t, dt*0.5, a);
		var c = evaluate(state, t, dt*0.5, b);
		var d = evaluate(state, t, dt, c);
		
		var velocity:Vector2D = calcTaylor(a.velocity, b.velocity, c.velocity, d.velocity);
		var acceleration:Vector2D = calcTaylor(a.acceleration, b.acceleration, c.acceleration, d.acceleration);
		
		state.position.plus(velocity.mult(dt));
		state.velocity.plus(acceleration.mult(dt));
	}
	
	static function integrateEuler(state:State, dt:Float, t:Float ) {
		var acc = acceleration(state, t + dt);
		state.position.plus(state.velocity.multNew(dt));
		state.velocity.plus(acc.multNew(dt));
	}

	public static function main() {
		var state:State = createEmptyState();
		var state2:State = createEmptyState();
		var t = 0.;
		var dt = 0.1;
		#if neko
			haxe.Log.trace = function(msg:Dynamic, ?infos) { neko.Lib.println(msg); }
		#end
		var px = new Vector2D(0,0);
		var pv = new Vector2D(0,0);
		var r = MathUtil.roundTo;
		while (t < 20) {
			integrateRK4(state, dt, t);
			integrateEuler(state2, dt, t);
			t += dt;
			trace(t+": "+state.position+" "+state.velocity+" vs "+state2.position+" "+state2.velocity);
		}
	}
	
	static function createEmptyState():State {
		return {
			position:new Vector2D(0.,0.),
			velocity:new Vector2D(0.,0.)
		};
	}
}